package org.nashua.tt151;

import org.nashua.tt151.dashboard.Dashboard;
import com.sun.squawk.debugger.Log;
import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Kinect;
import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Skeleton;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import java.util.TimerTask;

public class HeyJude extends SimpleRobot {
    public CANJaguar fl, fr, bl, br, st, sb, pu;
    public Servo dropper, pitch, yaw;
    public DigitalOutput led = new DigitalOutput(RobotMap.LED_SPIKE);
    public Dashboard dash;
    public boolean isLedOn = false;
    public ActionMap act;
    public void robotInit(){
        /* Initialize CANJaguars */
        fl = createJaguar(RobotMap.JAGUAR_FRONT_LEFT);
        fr = createJaguar(RobotMap.JAGUAR_FRONT_RIGHT);
        bl = createJaguar(RobotMap.JAGUAR_BACK_LEFT);
        br = createJaguar(RobotMap.JAGUAR_BACK_RIGHT);
        st = createJaguar(RobotMap.JAGUAR_SHOOTER_TOP);
        sb = createJaguar(RobotMap.JAGUAR_SHOOTER_BOTTOM);
        pu = createJaguar(RobotMap.JAGUAR_PICKUP);
        /* Initialize Servos */
        dropper = new Servo(RobotMap.SERVO_DROPPER);
        pitch = new Servo(RobotMap.SERVO_PITCH);
        yaw = new Servo(RobotMap.SERVO_YAW);
        /* Try To Establish Connection To ToughTechsDashboard */
        try {
            dash = new Dashboard(this);
            /* Update ToughTechsDashboard Periodically */
            new java.util.Timer().schedule(new TimerTask(){
                public void run() {
                    while (true) {
                        try {
                            if (fl!=null) updateCAN(RobotMap.JAGUAR_FRONT_LEFT,"Front Left",fl);
                            if (fr!=null) updateCAN(RobotMap.JAGUAR_FRONT_RIGHT,"Front Right",fr);
                            if (bl!=null) updateCAN(RobotMap.JAGUAR_BACK_LEFT,"Back Left",bl);
                            if (br!=null) updateCAN(RobotMap.JAGUAR_BACK_RIGHT,"Back Right",br);
                            if (st!=null) updateCAN(RobotMap.JAGUAR_SHOOTER_TOP,"Shooter Top",st);
                            if (sb!=null) updateCAN(RobotMap.JAGUAR_SHOOTER_BOTTOM,"Shooter Bottom",sb);
                            if (pu!=null) updateCAN(RobotMap.JAGUAR_PICKUP,"Pickup",pu);
                            if (dropper!=null) dash.updateInfo("Servo Dropper",""+format(dropper.get(),2));
                            if (pitch!=null) dash.updateInfo("Servo Pitch",""+format(pitch.get(),2));
                            if (yaw!=null) dash.updateInfo("Servo Yaw",""+format(yaw.get(),2));
                            if (led!=null) dash.updateInfo("LED",isLedOn?"On":"Off");
                            Timer.delay(0.1);
                        } catch (Exception e) {
                            Log.log("WARNING: Cannot Update ToughTechsDashboard - "+e.getMessage());
                        }
                    }
                }
                public String format(double num, double dec){
                    return ""+((int)num*(dec=MathUtils.pow(10,dec)))/dec;
                }
                public void updateCAN(int i, String name, CANJaguar jag){
                    try {
                        dash.updateCAN(i, name, format(jag.getX(),2)+"%", format(jag.getBusVoltage(),2)+"V", format(jag.getOutputVoltage(),2)+"V");
                    } catch (CANTimeoutException e) {
                        dash.updateCAN(i, name, "CAN TIMEOUT", "CAN TIMEOUT", "CAN TIMEOUT");
                    } catch (Exception e) {
                        Log.log("Warning: Cannot Update CAN Jaguar '"+name+"'");
                    }
                }
            }, 1);
        } catch (Exception e) {
            Log.log("Error: Failed To Establish Connection To ToughTechsDashboard - "+e.getMessage());
        }
        /* Create ActionMap */
        act = new ActionMap(this);
    }
    public void autonomous() {
        try {
            if (dash.requestInfo("Kinect").equals("1")) {
                KinectStick left = new KinectStick(1);
                KinectStick right = new KinectStick(2);
                while (isEnabled()&&isAutonomous()){
                    act.autoDrive(fl, fr, bl, br, -left.getY(), right.getY());
                    if (left.getRawButton(1)) act.shoot(st, sb, act.autoAim(pitch, yaw,0,led)); // HIGHEST
                    else if(left.getRawButton(2)) act.shoot(st, sb, act.autoAim(pitch, yaw,3,led)); // MIDDLE
                    if (left.getRawButton(7)) act.autoPickup(pu,1);
                    else if(left.getRawButton(8)) act.autoPickup(pu,0);
                    if (left.getRawButton(5)) act.autoDropper(dropper,RobotMap.V_SERVO_DROPPER_DOWN);
                    else if(left.getRawButton(6)) act.autoDropper(dropper, RobotMap.V_SERVO_DROPPER_UP);
                }
            } else {
                while (isEnabled()&&isAutonomous()){
                    act.shoot(st, sb, act.autoAim(pitch, yaw,0,led));
                }
            }
        } catch (Exception e) {
            Log.log("ERROR: Autonomous Failed - "+e.getMessage());
        }
    }
    public void operatorControl() {
        double power = 0.5;
        while (isEnabled()&&isOperatorControl()) {
            if (ActionMap.wasReleased(ActionMap.driver,RobotMap.DUALACTION_LEFT_TRIGGER,1)) power+=0.1;
            if (ActionMap.wasReleased(ActionMap.driver,RobotMap.DUALACTION_RIGHT_TRIGGER,1)) power-=0.1;
            act.manualDrive(power,fl,fr,bl,br,true,false);
            act.manualDropper(dropper);
            act.manualPickup(pu);
            act.manualLED(led);
            if (ActionMap.wasReleased(ActionMap.shooter,RobotMap.DUALACTION_LEFT_BUMPER,2)) act.manualAdjustShooter(pitch,yaw);
            if (ActionMap.wasReleased(ActionMap.shooter,RobotMap.DUALACTION_RIGHT_TRIGGER,2)) {
                int hoop = 0; // HIGHEST
                double x = ActionMap.shooter.getRawAxis(5);
                double y = ActionMap.shooter.getRawAxis(6);
                if (x<-0.5) hoop = 2; // MIDDLE LEFT
                else if (x>0.5) hoop = 4; // MIDDLE RIGHT
                else if (y<-0.5) hoop = 1; // TOP
                else if (y>0.5) hoop = 5; // BOTTOM
                act.manualShoot(pitch, yaw, st, sb, hoop, led);
            }
        }
    }
    /* HELPER METHODS */
    private static CANJaguar createJaguar(int slot){
        try {
            return new CANJaguar(slot,CANJaguar.ControlMode.kPercentVbus);
        } catch (Exception e){
            Log.log("ERROR: CANJaguar "+slot+" failed to initalize - "+e.getMessage());
            return null;
        }
    }
}
